#include "collisionBase.h"

#include <vector>

bool cmp_CollisionInstant( const CollisionInstant &l, const CollisionInstant &r )
{
	return l.TimePoint < r.TimePoint;
}

const Int16	TrianglePairTester::VFtestVertexMap[6][4] =
//    V    F0 F1 F2
{	{ 0,	3, 4, 5 },
	{ 1,	3, 4, 5 },
	{ 2,	3, 4, 5 },
	{ 3,	0, 1, 2 },
	{ 4,	0, 1, 2 },
	{ 5,	0, 1, 2 }	};

const Int16 TrianglePairTester::EEtestVertexMap[9][4] =
//E0:p0,p1 E1:p0,p1
{	{ 0, 1,    3, 4 },
	{ 0, 1,    3, 5 },
	{ 0, 1,    4, 5 },
	{ 0, 2,    3, 4 },
	{ 0, 2,    3, 5 },
	{ 0, 2,    4, 5 },
	{ 1, 2,    3, 4 },
	{ 1, 2,    3, 5 },
	{ 1, 2,    4, 5 }	};

// NOTE: TestDynamic returns a vector of CollisionInstant,
// each CollisionInstant in vector presents a collision,
// while the index of p0,...,p3 is LOCAL index, say: 
// first triangle: point 0,1,2. second: point 3,4,5.
vector<CollisionInstant>
TrianglePairTester::TestDynamic( const Float deltaT, Triangle3f &firstLoc, Triangle3f &firstV, Triangle3f &secondLoc, Triangle3f &secondV )
{
	vector<CollisionInstant>	res;

	Vector3f *locations[6];
	FOR(I,0,3)
		locations[I] = &firstLoc.cell[I];
	FOR(I,3,6)
		locations[I] = &secondLoc.cell[I - 3];
	Vector3f *velocities[6];
	FOR(I,0,3)
		velocities[I] = &firstV.cell[I];
	FOR(I,3,6)
		velocities[I] = &secondV.cell[I - 3];

	// the 6 VF test
	FOR(I,0,6) {
		Int32 p0_id = VFtestVertexMap[I][0];
		Int32 p1_id = VFtestVertexMap[I][1];
		Int32 p2_id = VFtestVertexMap[I][2];
		Int32 p3_id = VFtestVertexMap[I][3];
		vector<Float> tmp =
			DynamicVFtest(deltaT, *locations[p0_id], *locations[p1_id], *locations[p2_id], *locations[p3_id],
						  *velocities[p0_id],*velocities[p1_id],*velocities[p2_id],*velocities[p3_id]);
		// insert to result and keep the order
		vector<CollisionInstant> oldRes = res;
		Int32 newSz = SZ(oldRes) + SZ(tmp);
		res.resize(newSz);
		Int32 p_oldRes = 0, p_tmp = 0;
		FOR(J,0,newSz) {
			if(p_oldRes == SZ(oldRes)) {
				res[J].TimePoint = tmp[p_tmp];
				res[J].Type = COLLISION_TYPE_VF;
				res[J].p0 = p0_id;
				res[J].p1 = p1_id;
				res[J].p2 = p2_id;
				res[J].p3 = p3_id;
				++p_tmp;
			} else if(p_tmp == SZ(tmp)) {
				res[J] = oldRes[p_oldRes];
				++p_oldRes;
			} else if(oldRes[p_oldRes].TimePoint < tmp[p_tmp]) {
				res[J] = oldRes[p_oldRes];
				++p_oldRes;
			} else {
				res[J].TimePoint = tmp[p_tmp];
				res[J].Type = COLLISION_TYPE_VF;
				res[J].p0 = p0_id;
				res[J].p1 = p1_id;
				res[J].p2 = p2_id;
				res[J].p3 = p3_id;
				++p_tmp;
			}
		}
	}
	// the 9 EE test
	FOR(I,0,9) {
		Int32 p0_id = EEtestVertexMap[I][0];
		Int32 p1_id = EEtestVertexMap[I][1];
		Int32 p2_id = EEtestVertexMap[I][2];
		Int32 p3_id = EEtestVertexMap[I][3];
		vector<Float> tmp =
			DynamicEEtest(deltaT, *locations[p0_id], *locations[p1_id], *locations[p2_id], *locations[p3_id],
			*velocities[p0_id],*velocities[p1_id],*velocities[p2_id],*velocities[p3_id]);
		// insert to result and keep the order
		vector<CollisionInstant> oldRes = res;
		Int32 newSz = SZ(oldRes) + SZ(tmp);
		res.resize(newSz);
		Int32 p_oldRes = 0, p_tmp = 0;
		FOR(J,0,newSz) {
			if(p_oldRes == SZ(oldRes)) {
				res[J].TimePoint = tmp[p_tmp];
				res[J].Type = COLLISION_TYPE_EE;
				res[J].p0 = p0_id;
				res[J].p1 = p1_id;
				res[J].p2 = p2_id;
				res[J].p3 = p3_id;
				++p_tmp;
			} else if(p_tmp == SZ(tmp)) {
				res[J] = oldRes[p_oldRes];
				++p_oldRes;
			} else if(oldRes[p_oldRes].TimePoint < tmp[p_tmp]) {
				res[J] = oldRes[p_oldRes];
				++p_oldRes;
			} else {
				res[J].TimePoint = tmp[p_tmp];
				res[J].Type = COLLISION_TYPE_EE;
				res[J].p0 = p0_id;
				res[J].p1 = p1_id;
				res[J].p2 = p2_id;
				res[J].p3 = p3_id;
				++p_tmp;
			}
		}
	}
	return res;
}

// static triangle-triangle intersection test
// [Tomas Moller 97] : "A Fast Triangle-Triangle Intersection Test"
Boolean TrianglePairTester::TestStatic( Triangle3f &firstLoc, Triangle3f &secondLoc )
{
	// plane equation donation: A*x + B*y + C*z + D = 0
	//Float A, B, C, D;

	// step 1. "Compute plane equation of triangle 2"
	Vector3f v_2_01 = secondLoc.cell[1] - secondLoc.cell[0];
	Vector3f v_2_02 = secondLoc.cell[2] - secondLoc.cell[0];
	// (A, B, C) == n_2
	Vector3f n_2 = v_2_01.Cross(v_2_02);
	// D == D_2
	Float D_2 = -(n_2 * secondLoc.cell[0]);
	Float d_1_0 = firstLoc.cell[0] * n_2 + D_2;
	Float d_1_1 = firstLoc.cell[1] * n_2 + D_2;
	Float d_1_2 = firstLoc.cell[2] * n_2 + D_2;

	// coplanar check
	if(abs(d_1_0) < EPS_F && abs(d_1_1) < EPS_F && abs(d_1_2) < EPS_F) {
		Triangle3f_2D firstT, secondT;
        Vector3f abs_n_2;
        FOR(I,0,3) abs_n_2.cell[I] = abs(n_2.cell[I]);
		Int32 omitAxis = -1;	// 0:x, 1:y, 2:z
		if(abs_n_2.x > abs_n_2.y) {
			if(abs_n_2.x > abs_n_2.z)	omitAxis = 0;
			else				        omitAxis = 2;
		} else {
			if(abs_n_2.y > abs_n_2.z)	omitAxis = 1;
			else				        omitAxis = 2;
		}
        const static int selectAxis[3][2] = { {1,2}, {0,2}, {0,1} };
        FOR(I,0,3) {
            firstT.cell[I].cell[0] = firstLoc.cell[I].cell[ selectAxis[omitAxis][0] ];
            firstT.cell[I].cell[1] = firstLoc.cell[I].cell[ selectAxis[omitAxis][1] ];
            secondT.cell[I].cell[0] = secondLoc.cell[I].cell[ selectAxis[omitAxis][0] ];
            secondT.cell[I].cell[1] = secondLoc.cell[I].cell[ selectAxis[omitAxis][1] ];
        }
		return TriangleTriangleIntersection_2D(firstT, secondT);
	}

	// step 2. "Reject as trivial if all points of triangle 1 are on same side"
	Uint8 flag_1 = 0;
	if(d_1_0 > 0) flag_1 += 1 << 0;
	if(d_1_1 > 0) flag_1 += 1 << 1;
	if(d_1_2 > 0) flag_1 += 1 << 2;
	if(flag_1 == 0 || flag_1 == 7)
		return FALSE;

	// step 3. "Compute plane equation of triangle 1"
	Vector3f v_1_01 = firstLoc.cell[1] - firstLoc.cell[0];
	Vector3f v_1_02 = firstLoc.cell[2] - firstLoc.cell[0];
	// (A, B, C) == n_1
	Vector3f n_1 = v_1_01.Cross(v_1_02);
	// D == D_1
	Float D_1 = -(n_1 * firstLoc.cell[0]);
	Float d_2_0 = secondLoc.cell[0] * n_1 + D_1;
	Float d_2_1 = secondLoc.cell[1] * n_1 + D_1;
	Float d_2_2 = secondLoc.cell[2] * n_1 + D_1;

	// step 4. "Reject as trivial if all points of triangle 2 are on same side"
	Uint8 flag_2 = 0;
	if(d_2_0 > 0) flag_2 += 1 << 0;
	if(d_2_1 > 0) flag_2 += 1 << 1;
	if(d_2_2 > 0) flag_2 += 1 << 2;
	if(flag_2 == 0 || flag_2 == 7)
		return FALSE;

	// step 5. "Compute intersection line and project onto largest axis"
	// step 6. "Compute the intervals for each triangle"
	Vector3f D = n_1.Cross(n_2);
	FOR(I,0,3) D.cell[I] = abs(D.cell[I]);	// bug fixed at 2011-10-8
	// here comes the optimization strategy on paper
	Uint8 D_max_axis = 0;
	if(D.x > D.y) {
		if(D.x > D.z)	D_max_axis = 0;
		else			D_max_axis = 2;
	} else {
		if(D.y > D.z)	D_max_axis = 1;
		else			D_max_axis = 2;
	}
	const static Boolean positiveSingleTable[6] = { TRUE, TRUE, FALSE, TRUE, FALSE, FALSE };
	// arrange T1,T2's vertexes so that v1 is seperated with v0 & v2, by L
	Boolean positiveSingle;
	Vector3f *p_0, *p_1, *p_2;
    Float   *pd_0, *pd_1, *pd_2;
	// T1
	positiveSingle = positiveSingleTable[flag_1 - 1];
	if(d_1_0 > 0 == positiveSingle) {
		p_0 = &firstLoc.cell[1];
		p_1 = &firstLoc.cell[0];
		p_2 = &firstLoc.cell[2];
        pd_0 = &d_1_1;
        pd_1 = &d_1_0;
        pd_2 = &d_1_2;
	} else if(d_1_1 > 0 == positiveSingle) {
		p_0 = &firstLoc.cell[0];
		p_1 = &firstLoc.cell[1];
		p_2 = &firstLoc.cell[2];
        pd_0 = &d_1_0;
        pd_1 = &d_1_1;
        pd_2 = &d_1_2;
	} else {
		p_0 = &firstLoc.cell[0];
		p_1 = &firstLoc.cell[2];
		p_2 = &firstLoc.cell[1];
        pd_0 = &d_1_0;
        pd_1 = &d_1_2;
        pd_2 = &d_1_1;
	}
	Float pv_1_0 = p_0->cell[D_max_axis];
	Float pv_1_1 = p_1->cell[D_max_axis];
	Float pv_1_2 = p_2->cell[D_max_axis];
	Float t_1_1 = pv_1_0 + (pv_1_1 - pv_1_0) * (*pd_0 / (*pd_0 - *pd_1));
	Float t_1_2 = pv_1_2 + (pv_1_1 - pv_1_2) * (*pd_2 / (*pd_2 - *pd_1));
	// T2
	positiveSingle = positiveSingleTable[flag_2 - 1];
	if(d_2_0 > 0 == positiveSingle) {
		p_0 = &secondLoc.cell[1];
		p_1 = &secondLoc.cell[0];
		p_2 = &secondLoc.cell[2];
        pd_0 = &d_2_1;
        pd_1 = &d_2_0;
        pd_2 = &d_2_2;
	} else if(d_2_1 > 0 == positiveSingle) {
		p_0 = &secondLoc.cell[0];
		p_1 = &secondLoc.cell[1];
		p_2 = &secondLoc.cell[2];
        pd_0 = &d_2_0;
        pd_1 = &d_2_1;
        pd_2 = &d_2_2;
	} else {
		p_0 = &secondLoc.cell[0];
		p_1 = &secondLoc.cell[2];
		p_2 = &secondLoc.cell[1];
        pd_0 = &d_2_0;
        pd_1 = &d_2_2;
        pd_2 = &d_2_1;
	}
	Float pv_2_0 = p_0->cell[D_max_axis];
	Float pv_2_1 = p_1->cell[D_max_axis];
	Float pv_2_2 = p_2->cell[D_max_axis];
	Float t_2_1 = pv_2_0 + (pv_2_1 - pv_2_0) * (*pd_0 / (*pd_0 - *pd_1));
	Float t_2_2 = pv_2_2 + (pv_2_1 - pv_2_2) * (*pd_2 / (*pd_2 - *pd_1));

	// step 7. "Intersect the intervals"
	Float t_1_max = t_1_1, t_1_min = t_1_2;
	if(t_1_max < t_1_min) swap(t_1_max, t_1_min);
	Float t_2_max = t_2_1, t_2_min = t_2_2;
	if(t_2_max < t_2_min) swap(t_2_max, t_2_min);

	if(t_1_max < t_2_min) return FALSE;
	if(t_2_max < t_1_min) return FALSE;

	return TRUE;
}

Float TrianglePairTester::determinantValue( Vector3f &first, Vector3f &second, Vector3f &third )
{
	return Float((first.x * second.y * third.z  +  first.y * second.z * third.x  +  first.z * second.x * third.y) -
				 (first.x * second.z * third.y  +  first.y * second.x * third.z  +  first.z * second.y * third.x) );
}

vector<Float> TrianglePairTester::DynamicVFtest( Float deltaT, Vector3f &p0, Vector3f &p1, Vector3f &p2, Vector3f &p3, Vector3f &v0, Vector3f &v1, Vector3f &v2, Vector3f &v3 )
{
	vector<Float> res;
	// step 1 : coplanar condition
	vector<Float> coplanarTime = CoplanarTest(p0, p1, p2, p3, v0, v1, v2, v3);
	// step 2 : clamp candidate time
	ClampCoplanarCandidate(coplanarTime, deltaT);
	// step 3 : p0 inside triangle p1,p2,p3 condition
	Vector3f curP;			// V
	Triangle3f curTriangle;	// F
	Int32 sz = SZ(coplanarTime);
	FOR(I,0,sz) {
		Float t = coplanarTime[I];
		curP = p0 + v0 * t;
		curTriangle.cell[0] = p1 + v1 * t;
		curTriangle.cell[1] = p2 + v2 * t;
		curTriangle.cell[2] = p3 + v3 * t;
		if( PointInsideTriangle(curTriangle, curP) ) {
			// we have got a collision
			res.push_back(t);
		}
	}
	return res;
}

vector<Float> TrianglePairTester::DynamicEEtest( Float deltaT, Vector3f &p0, Vector3f &p1, Vector3f &p2, Vector3f &p3, Vector3f &v0, Vector3f &v1, Vector3f &v2, Vector3f &v3 )
{
	vector<Float> res;
	// step 1 : coplanar condition
	vector<Float> coplanarTime = CoplanarTest(p0, p1, p2, p3, v0, v1, v2, v3);
	// step 2 : clamp candidate time
	ClampCoplanarCandidate(coplanarTime, deltaT);
	// step 3 : edge p0,p1 intersect with edge p2,p3 condition
	Vector3f p0_start, p0_end;	// line0
	Vector3f p1_start, p1_end;	// line1
	Int32 sz = SZ(coplanarTime);
	FOR(I,0,sz) {
		Float t = coplanarTime[I];
		p0_start = p0 + v0 * t;
		p0_end	 = p1 + v1 * t;
		p1_start = p2 + v2 * t;
		p1_end	 = p3 + v3 * t;
		if( SegmentCross(p0_start, p0_end, p1_start, p1_end) ) {
			// we have got a collision
			res.push_back(t);
		}
	}
	return res;
}

vector<Float> TrianglePairTester::CoplanarTest( Vector3f &p0, Vector3f &p1, Vector3f &p2, Vector3f &p3, Vector3f &v0, Vector3f &v1, Vector3f &v2, Vector3f &v3 )
{
	Vector3f d_p01 = p0 - p1, d_p02 = p0 - p2, d_p03 = p0 - p3;
	Vector3f d_v01 = v0 - v1, d_v02 = v0 - v2, d_v03 = v0 - v3;

	// A*t^3 + B*t^2 + C*t + D = 0
	Float A = determinantValue(d_v01, d_v02, d_v03);

	Float B = determinantValue(d_v01, d_v02, d_p03) +
		      determinantValue(d_v01, d_p02, d_v03) + 
			  determinantValue(d_p01, d_v02, d_v03);

	Float C = determinantValue(d_p01, d_p02, d_v03) +
		      determinantValue(d_p01, d_v02, d_p03) +
			  determinantValue(d_v01, d_p02, d_p03);

	Float D = determinantValue(d_p01, d_p02, d_p03);

	return EquationSolver::SolveCubicEquation(A, B, C, D);
}

void TrianglePairTester::ClampCoplanarCandidate( vector<Float> &candidates, Float deltaT )
{
	Int16 p = 0, sz = SZ(candidates);
	FOR(I, 0, sz) {
		if(candidates[I] < 0) continue;
		if(candidates[I] > deltaT) break;
		candidates[p] = candidates[I];
		++p;
	}
	FOR(I, p, sz) candidates.pop_back();
}

llib::Boolean TrianglePairTester::PointInsideTriangle( Triangle3f &triangle, Vector3f &p )
{
	// barycentric coordinates to determine
	Vector3f v0 = triangle.cell[1] - triangle.cell[0];
	Vector3f v1 = triangle.cell[2] - triangle.cell[0];
	Vector3f v2 = p - triangle.cell[0];

	Float d00 = v0 * v0;
	Float d01 = v0 * v1;
	Float d11 = v1 * v1;
	Float d20 = v2 * v0;
	Float d21 = v2 * v1;
	Float denom = d00 * d11 - d01 * d01;

	Float v = (d11 * d20 - d01 * d21) / denom;
	if(v < 0 || v > 1) return FALSE;
	Float w = (d00 * d21 - d01 * d20) / denom;
	if(w < 0 || w > 1) return FALSE;
	Float u = 1.0f - v - w;
	if(u < 0 || u > 1) return FALSE;
	return TRUE;
}

llib::Boolean TrianglePairTester::PointInsideTriangle( Triangle3f_2D &triangle, Vector2f &p )
{
	// barycentric coordinates to determine
	Vector2f v0 = triangle.cell[1] - triangle.cell[0];
	Vector2f v1 = triangle.cell[2] - triangle.cell[0];
	Vector2f v2 = p - triangle.cell[0];

	Float d00 = v0 * v0;
	Float d01 = v0 * v1;
	Float d11 = v1 * v1;
	Float d20 = v2 * v0;
	Float d21 = v2 * v1;
	Float denom = d00 * d11 - d01 * d01;

	Float v = (d11 * d20 - d01 * d21) / denom;
	if(v < 0 || v > 1) return FALSE;
	Float w = (d00 * d21 - d01 * d20) / denom;
	if(w < 0 || w > 1) return FALSE;
	Float u = 1.0f - v - w;
	if(u < 0 || u > 1) return FALSE;
	return TRUE;
}

llib::Boolean TrianglePairTester::SegmentCross( Vector3f &p0_start, Vector3f &p0_end, Vector3f &p1_start, Vector3f &p1_end )
{
	Vector3f L0 = p0_end - p0_start;
	Vector3f L1 = p1_end - p1_start;

	// p1's start & end points are separated by line0
	Vector3f p0_start_p1_start = p1_start - p0_start;
	Vector3f p0_start_p1_end   = p1_end - p0_start;
	Vector3f v00 = L0.Cross(p0_start_p1_start);
	Vector3f v01 = L0.Cross(p0_start_p1_end);
	Float flag0 = v00 * v01;
	if(flag0 > 0) return FALSE;

	// p0's start & end points are separated by line1
	Vector3f p1_start_p0_start	= -p0_start_p1_start;
	Vector3f p1_start_p0_end	= p0_end - p1_start;
	Vector3f v10 = L1.Cross(p1_start_p0_start);
	Vector3f v11 = L1.Cross(p1_start_p0_end);
	Float flag1 = v10 * v11;
	if(flag1 > 0) return FALSE;

	return TRUE;
}

// intersection test for two triangle on plane
llib::Boolean TrianglePairTester::TriangleTriangleIntersection_2D( Triangle3f_2D &firstT, Triangle3f_2D &secondT )
{
	FOR(I,0,3)
		if(PointInsideTriangle(firstT,secondT.cell[I]))
			return TRUE;
	return FALSE;
}
